import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 定位到功能包的地址
    pkg_share = FindPackageShare(package='fishbot_cartographer').find('fishbot_cartographer')
    
    #=====================运行节点需要的配置=======================================================================
    # 是否使用仿真时间，我们用gazebo，这里设置成true
    # use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    # use_sim_time = 'true'
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    # 地图的分辨率
    resolution = LaunchConfiguration('resolution', default='0.05')
    # 地图的发布周期
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
    # 配置文件夹路径
    configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') )
    # 配置文件
    # configuration_basename = LaunchConfiguration('configuration_basename', default='simulate.lua')
    configuration_basename = LaunchConfiguration('configuration_basename', default='fishbot_2d.lua')
    fishbot_navigation2_dir = get_package_share_directory(
        'fishbot_navigation2')
    map_yaml_path = LaunchConfiguration('map', default=os.path.join(
        fishbot_navigation2_dir, 'maps', 'map06041133.yaml'))
    
    #=====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
    cartographer_node = Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=['-configuration_directory', configuration_directory,
                   '-configuration_basename', configuration_basename,
                #    '-initial_pose', '{to_trajectory_id=0, relative_pose={translation={x=10.0, y=2.0, z=0.0}, rotation={x=0.0, y=0.0, z=0.707, w=0.707}}}'
                #    '-load_state_filename',map_yaml_path
                   ])

    occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='occupancy_grid_node',
        name='occupancy_grid_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        # arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec, '-pure_localization',1])
        arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])
    
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        # arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen')

    #===============================================定义启动文件========================================================
    ld = LaunchDescription()
    ld.add_action(cartographer_node)
    # ld.add_action(occupancy_grid_node)
    # ld.add_action(rviz_node)

    return ld